from pyrobot.map.tkmap import TkMap
from math import cos, sin, pi, sqrt, tanh
import copy
from pyrobot.bean.map.mapstructure import MapStructure
from pyrobot.bean.map.cell import Cell

class GlobalView(TkMap):
   """
   GUI for visualizing the global perceptual space of a robot.
   """

   """
   ---------------------------------
   __init__: initialize a GPS window
   ----------------------------------
     cols: number of columns in map
     rows: number of rows in map
     value:
     widthMM: actual distance represented by a column (in MMs)
     heightMM: actual distance represented by a row (in MMs)
     title: window title
   """
   def __init__(self, cols=400, rows=400, value=0.5,
                widthMM=10000, heightMM=10000,
                title="Global Perceptual Space"):
      """ Pass in grid cols, grid cells, and total cols/rows in MM"""
      self.step = 0
      
      # 2000 is the max dist of a Pioneer sonar (in MMs) 
      self.range = 5000

      # create the map
      TkMap.__init__(self, cols, rows, value,
                     cols, rows,
                     widthMM, heightMM, title)
      
      # create the global map structure
      self.mapWidth = 500
      self.mapHeight = 500
      self.cellWidth = 50
      self.cellHeight = 50
      self.globalMap = MapStructure(self.mapWidth, self.mapHeight, self.cellWidth, self.cellHeight)
      self.cellMaxGap = 50
      # changed values of maps
      self.changedValues = []
      # keep track of obstacles
      self.obstacleList = []
      # center of view
      self.globalobstaclelist = []
      self.centerY = self.height/2 + 50
      self.centerX = self.width/2
      # robot position
      self.robotCell = Cell(0, 0, "robot")
      self.prevRobotCell = Cell(0, 0, "prevrobot")
      self.robotTrace = []
#      self.scaleX = self.width / self.mapWidth
#      self.scaleY = self.height / self.mapHeight
      self.scaleX = 3
      self.scaleY = 3
 
   def __plotCell__(self, cell):
       if self.globalMap.updateCellOrNot(cell):
           # a tricky thing???
           if cell.value == "obstacle":
               self.globalMap.addObstacle(cell)
           self.globalMap.setCellVaule(cell)
           #self.globalobstaclescells.extend(self.cellIndex2GraphCord(cell))
           self.changedValues = self.changedValues, cell
           #print(self.globalobstaclescells)
           #self.globalobstaclelist.append((cell.xIndex,cell.yIndex))
           #print(self.globalobstaclelist)
   
   def __localMap2GlobalMap__(self, localMap, robotCell):
       robotCellX = robotCell.xIndex
       robotCellY = robotCell.yIndex
       
       cellX = -localMap.getRows()+1
       while cellX < localMap.getRows():
           cellY = -localMap.getCols()+1
           while cellY < localMap.getCols():
               if localMap.getCellCoordinateValue(cellX, cellY) == "obstacle" or \
                  localMap.getCellCoordinateValue(cellX, cellY) == "free":
                   xIndex = cellX + robotCellX
                   yIndex = cellY + robotCellY
                   cell = Cell(xIndex, yIndex, localMap.getCellCoordinateValue(cellX, cellY))
                   self.__plotCell__(cell)
                   if localMap.getCellCoordinateValue(cellX, cellY) == "obstacle":
                       self.globalobstaclelist.append((cell.xIndex, cell.yIndex))
               cellY += 1
           cellX += 1
       
        
        
           
   """
   -------------------------------------------------------------------------
   updateFromLPS: update values based on LPS
   -------------------------------------------------------------------------
     lps: class containing sensor data
     robot: contains information about current robot
   """
   def updateFromLPS(self, lps, robot):
       # merge
#       self.localMap2GlobalMap(lps.localMap, robot)
       # clear the obstacle list
       
       del self.obstacleList[:]
       # get robot position
       x, y = self.globalMap.geoCord2CellCord(robot.x*1000, robot.y*1000)
       self.robotCell.xIndex = x
       self.robotCell.yIndex = y       
       # get the changes
       localMap = MapStructure(self.mapWidth, self.mapHeight, self.cellWidth, self.cellHeight)
       robotGlobalX = robot.x*1000
       robotGlobalY = robot.y*1000
       
#       self.first = True
#       if self.first == True:
#           print lps.localMap
#           self.first = False
       
       for i in range(lps.localMap.__len__()):
           x, y = lps.localMap[i] 
           # convert the local coordinate to a global one

           self.obstacleList.append(self.globalMap.geoCord2CellCord(x, y))    
           localMap.occupy(x, y)    
#           x = x + robotGlobalX
#           y = y + robotGlobalY
#           self.globalMap.occupy(x, y)
       
       # local to global
       self.__localMap2GlobalMap__(localMap, self.robotCell)
       #print(self.globalobstaclelist)
       #self.globalobstaclescells.extend(self.obstacleList)
       #print(self.globalMap.globalobstaclelist)
       #print(self.globalobstaclescells)
       #print(self.globalobstaclelist)
       
#       print "self.globalMap.posXnegY[1][1] ", self.globalMap.posXnegY[1][1]
#       print "self.globalMap.posXnegY[0][1] ", self.globalMap.posXnegY[0][1]

   def fitting(self):
       pass

   def cellIndex2GraphCord(self, cell):
       if cell.xIndex >= 0 and cell.yIndex >= 0:
           return (self.centerX-cell.yIndex*self.scaleX, self.centerY-cell.xIndex*self.scaleY)
       elif cell.xIndex >= 0 and cell.yIndex <= 0:
           return (self.centerX+(-cell.yIndex)*self.scaleX, self.centerY-cell.xIndex*self.scaleY)
       elif cell.xIndex <= 0 and cell.yIndex >= 0:
           return (self.centerX-cell.yIndex*self.scaleX, self.centerY+(-cell.xIndex)*self.scaleY)
       elif cell.xIndex <= 0 and cell.yIndex <= 0:
           return (self.centerX+(-cell.yIndex)*self.scaleX, self.centerY+(-cell.xIndex)*self.scaleY)
  
#   def __overlapTrace__(self, cell):
#       for tracepoint in self.robotTrace:
#           
  
   """
   -------------------------------------------------------------------------
   redraw: redraws the graphical map
   -------------------------------------------------------------------------
   """
#   def redraw(self, goalPos):
   def redraw(self):
      # traverse through updated points - redraw those points on map                      
      # draw the changes     
      while( len( self.changedValues ) ):
         self.changedValues, cell = self.changedValues
         if cell.value == "obstacle":
             x, y = self.cellIndex2GraphCord(cell)
             self.canvas.create_rectangle(x, y, x+2, y+2,
                                      width = 0,
                                      fill='black', tag = "old")
         else:
             x, y = self.cellIndex2GraphCord(cell)
             #self.globalobstaclescells.extend(x,y)
             self.canvas.create_rectangle(x, y, x+2, y+2,
                                      width = 0,
                                      fill='white', tag = "old")

      # insert trace point
      if self.prevRobotCell <> self.robotCell:
          # draw a line
#          print "draw a line"
#          prevX, prevY = self.cellIndex2GraphCord(self.prevRobotCell)
#          self.canvas.create_line([prevX+3, prevY+3, x+3, y+3], fill='green', width='2')
          self.robotTrace.append((self.prevRobotCell.xIndex, self.prevRobotCell.yIndex))
#          self.robotTrace.append((self.robotCell.xIndex, self.robotCell.yIndex))
      self.prevRobotCell = copy.deepcopy(self.robotCell)

      
#      startX = startY = 0.0
      nextX = nextY = 0.0
      if self.robotTrace.__len__() <> 0:
          startX, startY = self.robotTrace[0]
          for (nextX, nextY) in self.robotTrace[1:]:               
              # draw one point
              prevCell = Cell(startX, startY, "trace")
              prevX, prevY = self.cellIndex2GraphCord(prevCell)
              self.canvas.create_rectangle(prevX, prevY, prevX+6, prevY+6,
                                      width = 0,
                                      fill='red', tag = "trace")
              currentCell = Cell(nextX, nextY, "trace")
              currentX, currentY = self.cellIndex2GraphCord(currentCell)
#              self.canvas.create_rectangle(currentX, currentY, currentX+6, currentY+6,
#                                          width = 0,
#                                          fill='red', tag = "trace")
              # draw a line
              self.canvas.create_line([prevX+3, prevY+3, currentX+3, currentY+3], fill='green', width='2')
              
              startX = nextX
              startY = nextY
              
#              print "(nextX, nextY) ", (nextX, nextY)
          # draw the end point
#          endCell = Cell(startX, startY, "trace")
#          prevX, prevY = self.cellIndex2GraphCord(endCell)
#          self.canvas.create_rectangle(prevX, prevY, prevX+6, prevY+6,
#                                      width = 0,
#                                      fill='red', tag = "trace")

      # draw the robot
#      print "draw robot"
#      self.canvas.delete("robot")
      robotX = robotY = 0
      robotX, robotY = self.cellIndex2GraphCord(self.robotCell)
      self.canvas.create_rectangle(robotX, robotY, robotX+6, robotY+6,
                                          width = 0,
                                          fill='red', tag = "robot")
      # draw the line to robot
      if self.robotTrace.__len__() <> 0:
#          print "prevX ", prevX
#          print "robotX ", robotX
          prevCell = Cell(nextX, nextY, "trace")
          prevX, prevY = self.cellIndex2GraphCord(prevCell)
          self.canvas.create_line([prevX+3, prevY+3, robotX+3, robotY+3], fill='green', width='2')

            
if __name__ == '__main__':
    gps = GlobalView(50, 50)
    gps.application = 1
    gps.mainloop()

